#include <Physics.h>

#include <BaseObject.h>

#include <fstream>

PhysicsManager::PhysicsManager()
{
	world = 0;
	paused = 0;
	oneStep = 0;
	collisions = 1;
	debugDrawEnabled = 0;
	External::physics = this;
	debugMode = 0;
}

PhysicsManager::~PhysicsManager()
{
}

void PhysicsManager::Init()
{
	b2Vec2 gravity = b2Vec2(0, 0);
	world = new b2World(gravity);

	world->SetDebugDraw(&debugDraw);
	debugDraw.SetFlags(b2Draw::e_shapeBit);

	collisionListener.Init( &collisionPairs );
	world->SetContactListener( &collisionListener );

	/* //messing around...
	b2BodyDef groundBodyDef;
	groundBodyDef.position.Set(50,50);
	b2Body* groundBody = world->CreateBody(&groundBodyDef);
	b2PolygonShape groundBox;
	groundBox.SetAsBox(50,10);
	groundBody->CreateFixture(&groundBox,0);

	
	b2BodyDef groundBodyDef2;
	groundBodyDef2.position.Set(100,0);
	b2Body* groundBody2 = world->CreateBody(&groundBodyDef2);
	b2PolygonShape groundBox2;
	groundBox2.SetAsBox(50,10);
	groundBody2->CreateFixture(&groundBox2,0);
	*/


	/*
	//make some test stuff
	b2BodyDef groundBodyDef;
	groundBodyDef.position.Set(0,-10);
	b2Body* groundBody = world->CreateBody(&groundBodyDef);
	b2PolygonShape groundBox;
	groundBox.SetAsBox(50,10);
	groundBody->CreateFixture(&groundBox,0);

	b2BodyDef bodyDef;
	bodyDef.type = b2_dynamicBody;
	bodyDef.position.Set(0,5);
	b2Body* body = world->CreateBody(&bodyDef);
	b2PolygonShape dynamicBox;
	dynamicBox.SetAsBox(2,2);
	b2FixtureDef fixtureDef;
	fixtureDef.shape = &dynamicBox;
	fixtureDef.density = 1;
	fixtureDef.friction = 0.3f;
	body->CreateFixture(&fixtureDef);
	btest = body;
	*/
}

void PhysicsManager::Update(float dt)
{
	if(paused)
		return;

	if(oneStep)
	{
		oneStep = false;
		paused = true;
	}

	world->Step(0.016f, 6, 2);
	//world->Step(dt * 0.1f, 6, 2);
	world->ClearForces();

	if(collisions)
		ProcessCollisions();
}
		
void PhysicsManager::DoDebugDraw()
{
	if(debugDrawEnabled)
	{
		//debug draw physics
		debugDraw.DrawString(5, 15, "Hello! Debugging Enabled");
		//debugDraw.DrawString(5, 30, "mx = %f", app.mx);
		//debugDraw.DrawString(5, 45, "my = %f", app.my);
		//debugDraw.DrawPoint(b2Vec2( -1 + app.mx/400, 1 -app.my/300), 10.0, b2Color(1,0,0) );

		world->DrawDebugData();
	}
}

void PhysicsManager::Cleanup()
{
}

void PhysicsManager::SetPause(truth state)
{
	paused = state;
	oneStep = false;
}

void PhysicsManager::SetCollisions(truth state)
{
	collisions = state;
}
		
void PhysicsManager::SetDebugMode(truth state)
{
	if(state)
		debugDrawEnabled = true;
	else
		debugDrawEnabled = false;
	debugMode = state;
}

void PhysicsManager::DoOneStep()
{
	paused = false;
	oneStep = true;
}

std::vector<vec2> PhysicsManager::LoadBoundingBox(std::string filename)
{
	if( HasBoundingBox( filename ) )
		return boundingBoxes[ loadedBBs[ filename ] ];

	std::vector< vec2 > points;
	std::fstream fs(filename.c_str(), std::ios::in);

	if( !fs.is_open() )
	{
		std::cerr<<"Error attempting to load bounding box: ";
		std::cerr<<"FILE NOT FOUND: "<<filename <<"\n";
		return points;
	}
	unsigned totalPoints = 0;
	unsigned readPoints = 0;

	fs >> totalPoints;

	while( fs.good() )
	{
		vec2 point;
		fs >> point.x;
		fs >> point.y;
		points.push_back( point );
		readPoints++;
	}

	if( readPoints != totalPoints )
	{
		std::cerr<<"Error bounding box Load failed: totalPoints != amount of points read from file!\n";
		points.clear();
		return points;
	}
		
	if( !AddBoundingBox( points, filename ) )
	{
		std::cerr<<"Error: failed to add graphic to system!\n";
		points.clear();
		return points;
	}

	return points;
}

truth PhysicsManager::AddBoundingBox( std::vector<vec2>& points, std::string filename )
{
	if( points.empty() )
	{
		std::cerr<<"error cannot add empty set of points\n";
		return 0;
	}

	unsigned location = boundingBoxes.size();
	boundingBoxes.push_back( points );
	loadedBBs[ filename ] = location;

	return 1;
}
			
truth PhysicsManager::HasBoundingBox( std::string filename )
{
	if( loadedBBs.find( filename ) != loadedBBs.end() )
	{
		return 1;
	}
	return 0;
}
		
void PhysicsManager::ProcessCollisions()
{
	for(unsigned i = 0; i < collisionPairs.size(); ++i)
	{
		collisionPairs[i].first->DoCollision( collisionPairs[i].second );
		collisionPairs[i].second->DoCollision( collisionPairs[i].first );
	}
	collisionPairs.clear();
}

